use std::net::TcpStream;
use std::io::{Write, Read};
use std::collections::HashMap;
use crate::json_parse::ECU_MAP;
use crate::{doip_layer, doip_layer::UdsMsg};
use rand::Rng;

//定义消息类型
const MSG_PLAD_TYPE_NACK: u16                           = 0x0000;
//const MSG_PLAD_TYPE_DISCOVERY_REQ: u16                  = 0x0001;
//const MSG_PLAD_TYPE_DISCOVERY_WITH_VIN_REQ: u16         = 0x0003;
//const MSG_PLAD_TYPE_DISCOVERY_RSP: u16                  = 0x0004;
const MSG_PLAD_TYPE_ROUTE_ACTIE_REQ: u16                = 0x0005;
const MSG_PLAD_TYPE_ROUTE_ACTIE_RSP: u16                = 0x0006;
//const MSG_PLAD_TYPE_ALIVE_CHECK_REQ: u16                = 0x0007;
//const MSG_PLAD_TYPE_ALIVE_CHECK_RSP: u16                = 0x0008;
const MSG_PLAD_TYPE_DIAG_MSG: u16                       = 0x8001;
const MSG_PLAD_TYPE_DIAG_ACK: u16                       = 0x8002;
//const MSG_PLAD_TYPE_DIAG_NACK: u16                      = 0x8003;

trait DoipService {
    fn handle(&self, data: &[u8]) -> Vec<u8>;
}

impl<F> DoipService for F 
    where F: Fn(&[u8]) -> Vec<u8> + 'static,
{
    fn handle(&self, data: &[u8]) -> Vec<u8> {
        self(data)
    }
}

struct DealDoipReq {
    pub service_map: HashMap<u8, Box<dyn DoipService>>,
}

impl DealDoipReq {
    pub fn new() -> DealDoipReq {
        let mut service_map = HashMap::new();
        service_map.insert(0x10, Box::new(service_10) as Box<dyn DoipService>);
        service_map.insert(0x11, Box::new(service_11) as Box<dyn DoipService>);
        service_map.insert(0x14, Box::new(service_14) as Box<dyn DoipService>);
        service_map.insert(0x19, Box::new(service_19) as Box<dyn DoipService>);
        service_map.insert(0x22, Box::new(service_22) as Box<dyn DoipService>);
        service_map.insert(0x27, Box::new(service_27) as Box<dyn DoipService>);
        service_map.insert(0x28, Box::new(service_28) as Box<dyn DoipService>);
        service_map.insert(0x2e, Box::new(service_2e) as Box<dyn DoipService>);
        service_map.insert(0x2f, Box::new(service_2f) as Box<dyn DoipService>);
        service_map.insert(0x31, Box::new(service_31) as Box<dyn DoipService>);
        service_map.insert(0x34, Box::new(service_34) as Box<dyn DoipService>);
        service_map.insert(0x35, Box::new(service_35) as Box<dyn DoipService>);
        service_map.insert(0x36, Box::new(service_36) as Box<dyn DoipService>);
        service_map.insert(0x37, Box::new(service_37) as Box<dyn DoipService>);
        service_map.insert(0x3e, Box::new(service_3e) as Box<dyn DoipService>);
        service_map.insert(0x85, Box::new(service_85) as Box<dyn DoipService>);
        service_map.insert(0xFF, Box::new(unsupported_service) as Box<dyn DoipService>);
        DealDoipReq { 
            service_map,
        }
    }
}

fn messages_to_str(buf: &[u8]) -> String
{
    let len = if buf.len() > 16 {16} else {buf.len()};
    let mut str = String::new();
    for i in 0..len {
        str.push_str(&format!("{:02X}", buf[i]));
        if i != len - 1 {
            str.push_str(" ");
        }
    }
    return str;
}

pub fn handle_request(mut stream: TcpStream) -> Result<(), Box<dyn std::error::Error>>
{
    let mut buffer = [0; 4096];
    let doip_diag = DealDoipReq::new();

    loop {
        buffer.fill(0); //接收前清空
        let result = stream.read(&mut buffer);
        let rcv_len = match result {
            Ok(n) => {
                if n == 0 {
                    return Err("Client disconnected".into());
                }
                n
            }
            Err(e) => {
                return Err(e.into());
            }
        };

        let mut index: usize = 0;
        while index < rcv_len {
            let (plad_type, plad_lenth) = doip_layer::parse_header(&buffer[index..rcv_len]);
            let mut rsp_data = Vec::new();

            if plad_type == 0 {
                index += 1;
                continue;
            } else {
                index += 8;
                if rcv_len < (plad_lenth as usize) + index {
                    break;
                } else {
                    rsp_data.clear();
                    let plad_data = &buffer[index..index + plad_lenth as usize];
                    println!("type: {:02X}, len: {:04X}, data: {:X?}", plad_type, plad_lenth, messages_to_str(plad_data));

                    match plad_type {
                        MSG_PLAD_TYPE_ROUTE_ACTIE_REQ => {
                            rsp_data = handle_route_active(plad_data);
                        }
                        MSG_PLAD_TYPE_DIAG_MSG => {
                            rsp_data = handle_diag_msg(plad_data, &stream, &doip_diag);
                        }
                        _ => {
                            eprintln!("unsupported message type: {:X}", plad_type);
                            rsp_data.clear();
                        }
                    }

                    if rsp_data.len() > 0 {
                        println!("send: {:02X?}", rsp_data);
                        stream.write_all(&rsp_data)?;
                    }

                    index += plad_lenth as usize;
                }
            }
        }
    }
}

fn handle_route_active(buf: &[u8]) -> Vec<u8>
{
    if buf.len() < 4 {
        return doip_layer::create_rsp_msg(MSG_PLAD_TYPE_NACK, &[0x04]);
    }

    let target_address = ((buf[0] as u16) << 8) | buf[1] as u16;
    let source_address = 0x1700;    //路由激活时，doip帧中不存在目标地址
    let plad_data = doip_layer::pack_payload(target_address, source_address, &[0x10, 0x00, 0x00, 0x00, 0x00]);
    return doip_layer::create_rsp_msg(MSG_PLAD_TYPE_ROUTE_ACTIE_RSP, &plad_data);
}

fn handle_diag_msg(buf: &[u8], mut stream: &TcpStream, diag: &DealDoipReq) -> Vec<u8>
{
    if buf.len() < 5 {
        return doip_layer::create_rsp_msg(MSG_PLAD_TYPE_NACK, &[0x04]);
    }

    //ACK
    let target_address = ((buf[0] as u16) << 8) | buf[1] as u16;
    let source_address = ((buf[2] as u16) << 8) | buf[3] as u16;
    let payload_data = doip_layer::pack_payload(target_address, source_address, &[0x00]);
    stream.write_all(&doip_layer::create_rsp_msg(MSG_PLAD_TYPE_DIAG_ACK, &payload_data)).unwrap_or_else(|err| {
        eprintln!("Error writing to stream: {}", err);
    });

    //RSP
    let usr_data;
    let payload;
    if diag.service_map.contains_key(&buf[4]) {
        println!("service: 0x{:02X}", buf[4]);
        //0x22服务需要单独处理
        if buf[4] == 0x22 {
            usr_data = diag.service_map.get(&buf[4]).unwrap().handle(&buf[..]);
        } else {
            usr_data = diag.service_map.get(&buf[4]).unwrap().handle(&buf[4..]);
        }
    } else {
        eprintln!("unsupported service: 0x{:02X}", buf[4]);
        usr_data = diag.service_map.get(&0xFF).unwrap().handle(&buf[4..]);
    }

    if usr_data.len() > 0 {
        //处理功能寻址
        if buf[2] == 0xE4 {
            println!("function addressing: 0x{:04X}", source_address);
            return doip_layer::function_address_frame_resp(MSG_PLAD_TYPE_DIAG_MSG, target_address, &usr_data)
        }
        payload = doip_layer::pack_payload(target_address, source_address, &usr_data);
        return doip_layer::create_rsp_msg(MSG_PLAD_TYPE_DIAG_MSG, &payload);
    } else {
        let mut dat = Vec::new();
        dat.clear();
        return dat;
    }
}

fn service_10(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);

    if data.len() < 2 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    if data[1] == 0x01 || data[1] == 0x02 || data[1] == 0x03 {
        uds_msg.create_rsp_msg(&[data[1], 0x00, 0x32, 0x00, 0x32]);
    }
    else {
        eprintln!("service 10 subfunction error: {}", data[1]);
        uds_msg.nrc = 0x12;
        uds_msg.create_nrc_msg();
    }
    
    return rsp;
}

fn service_11(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);

    if data.len() != 2 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    if data[1] == 0x01 || data[1] == 0x03 {
        uds_msg.create_rsp_msg(&[data[1]]);
    } else {
        eprintln!("service 11 subfunction error: {}", data[1]);
        uds_msg.nrc = 0x12;
        uds_msg.create_nrc_msg();
    }

    return rsp;
}

fn service_14(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);

    if data.len() != 4 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    uds_msg.create_rsp_msg(&[]);

    return rsp;
}

fn service_19(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);

    if data.len() > 3 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    let subfunction = data[1];
    if subfunction == 0x01 {
        uds_msg.create_rsp_msg(&[subfunction, 0x00, 0x00, 0x00]);
    } else if subfunction == 0x02 {
        uds_msg.create_rsp_msg(&[subfunction, 0xFF]);
    } else if subfunction == 0x03 {
        uds_msg.create_rsp_msg(&[subfunction]);
    } else if subfunction == 0x04 {
        uds_msg.create_rsp_msg(&[subfunction, 0xFF, 0xFF, 0xFF, 0xFF]);
    } else {
        eprintln!("service 19 subfunction error: {}", data[1]);
        uds_msg.nrc = 0x12;
        uds_msg.create_nrc_msg();
    }
    return rsp;
}

fn service_22(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[4], &mut rsp);
    let addr = ((data[2] as u16) << 8) | data[3] as u16;
    let mut pn = "Bxxxxxxx";
    let mut sw = "Sxxxxxx";
    let ecu_map = ECU_MAP.lock().unwrap();
    let ecu_info = ecu_map.get_ecu(addr);
    if ecu_info.is_some() {
        pn = ecu_info.unwrap().get_pn();
        sw = ecu_info.unwrap().get_sw();
    }

    if data.len() <= 5 {
        eprintln!("Invalid data length. {}: {:X?}", data.len(), data);
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }
    let did = ((data[5] as u16) << 8) | data[6] as u16;
    let mut combined_data = vec![data[5], data[6]];
    match did {
        0xF187 => {
            combined_data.extend_from_slice(pn.as_bytes());
            uds_msg.create_rsp_msg(&combined_data);
        }
        0xF195 => {
            combined_data.extend_from_slice(sw.as_bytes());
            uds_msg.create_rsp_msg(&combined_data);
        }
        0xF1BC => { //获取日志存储信息
            let log_num = 0u8;
            let address: [u8; 4] = [0x00, 0x08, 0x00, 0x00];
            let size: [u8; 4] = [0x00, 0x04, 0xB0, 0x00];   //每次传1024B，需要传300次
            combined_data.push(log_num);
            combined_data.extend_from_slice(&address);
            combined_data.extend_from_slice(&size);
            uds_msg.create_rsp_msg(&combined_data);
        }
        _ => {
            eprintln!("Invalid did: {:X}", did);
            uds_msg.nrc = 0x31;
            uds_msg.create_nrc_msg();
        }
    }
    return rsp;
}

fn service_27(data: &[u8]) -> Vec<u8>
{
    let mut rng = rand::thread_rng();
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);  

    if data.len() < 2 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    let subfunction = data[1];
    if subfunction == 0x01  || subfunction == 0x11{
        uds_msg.create_rsp_msg(&[subfunction, rng.gen_range(1..=0xFF), rng.gen_range(1..=0xFF), rng.gen_range(1..=0xFF), rng.gen_range(1..=0xFF)]);
    } else if subfunction == 0x02 || subfunction == 0x12 {
        uds_msg.create_rsp_msg(&[subfunction]);
    } else {
        eprintln!("service 27 subfunction error: {}", data[1]);
        uds_msg.nrc = 0x31;
        uds_msg.create_nrc_msg();
    }
    return rsp;
}

fn service_28(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);

    if data.len() != 3 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    uds_msg.create_rsp_msg(&[data[1]]);
    return rsp;
}

fn service_2e(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);

    if data.len() < 4 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    uds_msg.create_rsp_msg(&[data[1], data[2]]);
    return rsp;
}

fn service_2f(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);

    if data.len() < 4 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    uds_msg.create_rsp_msg(&data[1..]);//跳过服务id, did+iocp+cs原样返回
    return rsp;
}

fn service_31(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);
    if data.len() < 3 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    let routine_id = data[1];
    let routine_type = (data[2] as u16) << 8 | data[3] as u16;

    if routine_type == 0xFF00 || routine_type == 0xFF01 || routine_type == 0x0203 || routine_type == 0x0202 {
        uds_msg.create_rsp_msg(&[routine_id, data[2], data[3], 0x00]);
    } else {
        uds_msg.create_rsp_msg(&data[1..]); //其它DID数据原样返回
    }
    return rsp;
}

fn service_34(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);
    if data.len() < 3 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    uds_msg.create_rsp_msg(&[0x20, 0x04, 0x02]);
    return rsp;
}

fn service_35(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);
    if data.len() < 3 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    if data[1] == 0x00 &&  data[2] == 0x44 {
        uds_msg.create_rsp_msg(&[0x20, 0x04, 0x02]);    //0x0402为maxNumberOfBlockLength
    } else {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
    }
    return rsp;
}

fn service_36(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);
    if data.len() < 2 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    uds_msg.create_rsp_msg(&[data[1]]);
    return rsp;
}

fn service_37(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);

    uds_msg.create_rsp_msg(&[]);
    return rsp;    
}

fn service_3e(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);
    if data.len() != 2 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    let inb = (data[1] & 0x80) >> 7;
    if inb == 1 {
        rsp.clear();
    } else {
        uds_msg.create_rsp_msg(&[data[1]]);
    }
    return rsp;
}

fn service_85(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);
    if data.len() != 2 {
        uds_msg.nrc = 0x13;
        uds_msg.create_nrc_msg();
        return rsp;
    }

    uds_msg.create_rsp_msg(&[data[1]]);
    return rsp;
}

fn unsupported_service(data: &[u8]) -> Vec<u8>
{
    let mut rsp = Vec::new();
    let mut uds_msg = UdsMsg::new(data[0], &mut rsp);
    uds_msg.nrc = 0x10;
    uds_msg.create_nrc_msg();
    return rsp;
}